#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stddef.h>
#include <unistd.h>

#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"

#include "globals.h"
#include "lcd.h"
#include "pwms.h"
#include "pwm_regs.h"
#include "pwm_routines.h"

static unsigned long assigned_values[PWM_COUNT];

int InitialisePWMs(void)
{
    int rotate_ret, shoulder_ret, elbow_ret, gripper_ret;
    int i;
    
    /* Initialise the PWMs */
    rotate_ret   = altera_avalon_pwm_init(ROTATEPWM_BASE, 
                                          PWM_CLOCK_DIV, 
                                          PWM_CENTRE);
                                          
    shoulder_ret = altera_avalon_pwm_init(SHOULDERPWM_BASE, 
                                          PWM_CLOCK_DIV, 
                                          PWM_CENTRE);
                                          
    elbow_ret    = altera_avalon_pwm_init(ELBOWPWM_BASE, 
                                          PWM_CLOCK_DIV, 
                                          PWM_CENTRE);
                                          
    gripper_ret  = altera_avalon_pwm_init(GRIPPERPWM_BASE, 
                                          PWM_CLOCK_DIV, 
                                          PWM_CENTRE);
    
    if ((rotate_ret == ALTERA_AVALON_PWM_OK) && 
        (shoulder_ret == ALTERA_AVALON_PWM_OK) && 
        (elbow_ret == ALTERA_AVALON_PWM_OK) && 
        (gripper_ret == ALTERA_AVALON_PWM_OK))
    { 
        /* Enable PWMs if initialisation was successful */
        rotate_ret   = altera_avalon_pwm_enable(ROTATEPWM_BASE);
        shoulder_ret = altera_avalon_pwm_enable(SHOULDERPWM_BASE);
        elbow_ret    = altera_avalon_pwm_enable(ELBOWPWM_BASE);
        gripper_ret  = altera_avalon_pwm_enable(GRIPPERPWM_BASE);
        
        if ((rotate_ret != ALTERA_AVALON_PWM_OK) || 
            (shoulder_ret != ALTERA_AVALON_PWM_OK) || 
            (elbow_ret != ALTERA_AVALON_PWM_OK) || 
            (gripper_ret != ALTERA_AVALON_PWM_OK))
        {
            printf("Base: %d\r\n, Shoulder: %d\r\n, Elbow: %d\r\n, Gripper: %d\r\n", rotate_ret, shoulder_ret, elbow_ret, gripper_ret);
            
            return -1;
        }
        
        for (i = 0; i < PWM_COUNT; i++)
        {
            assigned_values[i] = PWM_MIN;
        }
        
        return 0;
    }
    
    printf("Base: %d\r\n, Shoulder: %d\r\n, Elbow: %d\r\n, Gripper: %d\r\n", rotate_ret, shoulder_ret, elbow_ret, gripper_ret);
    
    return -1;
}

int GetIncrementValue(const unsigned long start_value, 
                      const unsigned long new_value)
{
    if (start_value < new_value)
    {
        return PWM_INC;
    }
    
    return PWM_DEC;
}

int EnablePWM(unsigned int pwm)
{
    return (altera_avalon_pwm_enable(pwm) == ALTERA_AVALON_PWM_OK);
}

int EnablePWMs(void)
{   
    return ((EnablePWM(ROTATEPWM_BASE)) && 
            (EnablePWM(SHOULDERPWM_BASE)) && 
            (EnablePWM(ELBOWPWM_BASE)) && 
            (EnablePWM(GRIPPERPWM_BASE)));
}

int DisablePWM(unsigned int pwm)
{
    return (altera_avalon_pwm_disable(ROTATEPWM_BASE) == ALTERA_AVALON_PWM_OK);
}

int DisablePWMs(void)
{
    return ((DisablePWM(ROTATEPWM_BASE)) && 
            (DisablePWM(SHOULDERPWM_BASE)) && 
            (DisablePWM(ELBOWPWM_BASE)) && 
            (DisablePWM(GRIPPERPWM_BASE)));
}

int GetPWMId(const unsigned int pwm)
{
    switch (pwm)
    {
        case ROTATEPWM_BASE:
            return PWM_ID_ROTATE;
        case SHOULDERPWM_BASE:
            return PWM_ID_SHOULDER;
        case ELBOWPWM_BASE:
            return PWM_ID_ELBOW;
        case GRIPPERPWM_BASE:
            return PWM_ID_GRIPPER;
        default:
            break;
    }
    
    return -1;
}

void SendValueToPWM(unsigned int pwm, unsigned long value)
{
    int internal_pwm = 0;
    unsigned long current_value, end_value;
    int inc;
    int i = 0;
    
    internal_pwm = GetPWMId(pwm);
    if (value != assigned_values[internal_pwm])
    {
        current_value = assigned_values[internal_pwm];
        inc = GetIncrementValue(current_value, value);
        end_value = value + inc;
        
        LOOP_FOREVER()
        {
            current_value += inc;
            i++;
            altera_avalon_pwm_change_duty_cycle(pwm, current_value);
            
            if (((inc > 0) && (current_value > end_value)) || 
                ((inc < 0) && (current_value < end_value)))
            {
                break;
            }
        }
        
        assigned_values[internal_pwm] = current_value;
    }
    
    return;
}

void CentrePWM(unsigned int pwm)
{
    SendValueToPWM(pwm, PWM_CENTRE);
    return;
}

void CentrePWMs(void)
{
    CentrePWM(ROTATEPWM_BASE);
    CentrePWM(SHOULDERPWM_BASE);
    CentrePWM(ELBOWPWM_BASE);
    CentrePWM(GRIPPERPWM_BASE);
    return;
}

void HomePWMs(void)
{
    CentrePWM(ROTATEPWM_BASE);
    SendValueToPWM(SHOULDERPWM_BASE, PWM_CENTRE);
    SendValueToPWM(ELBOWPWM_BASE, PWM_CENTRE);
    SendValueToPWM(GRIPPERPWM_BASE, PWM_MIN);
    
    return;
}

void IncPWM(unsigned int pwm, unsigned short value)
{
    int internal_pwm = GetPWMId(pwm);
    
    if (internal_pwm > -1)
    {
        SendValueToPWM(pwm, assigned_values[internal_pwm] + value);
    }
    
    return;
}

void DecPWM(unsigned int pwm, unsigned short value)
{
    int internal_pwm = GetPWMId(pwm);
    
    if (internal_pwm > -1)
    {
        SendValueToPWM(pwm, assigned_values[internal_pwm] - value);
    }
    
    return;
}

void TestPWMs(void)
{   
    HomePWMs();
    
    /* Base first */
    SendValueToPWM(ROTATEPWM_BASE, PWM_MIN);
    SendValueToPWM(ROTATEPWM_BASE, PWM_MAX);
    CentrePWM(ROTATEPWM_BASE);
    
    /* Shoulder */
    SendValueToPWM(SHOULDERPWM_BASE, PWM_MIN);
    SendValueToPWM(SHOULDERPWM_BASE, PWM_MAX);
    
    /* Elbow */
    SendValueToPWM(ELBOWPWM_BASE, PWM_MIN);
    SendValueToPWM(ELBOWPWM_BASE, PWM_MAX);
    
    /* Gripper */
    SendValueToPWM(GRIPPERPWM_BASE, PWM_MIN);
    SendValueToPWM(GRIPPERPWM_BASE, PWM_MAX);
    CentrePWM(GRIPPERPWM_BASE);
    
    return;
}

void InvertGripperPWM(void)
{
    int internal_pwm = GetPWMId(GRIPPERPWM_BASE);
    unsigned long int current_value;
    
    if (internal_pwm > -1)
    {
        current_value = assigned_values[internal_pwm];
        if (current_value >= (PWM_MAX - PWM_INC))
        {
            SendValueToPWM(GRIPPERPWM_BASE, PWM_MIN);
            WriteLCD(">< GRIPPER\n><  CLOSE");
        }
        else
        {
            SendValueToPWM(GRIPPERPWM_BASE, PWM_MAX);
            WriteLCD("<> GRIPPER\n<>  OPEN");
        }
    }
    
    return;
}
